clear;clc;
bag=rosbag('C:\Users\10071\Desktop\rosbag2\uav1_2021-04-29-16-03-01.bag.active');%读取整个包


%/uav1/mavros/vision_pose/pose       %原始数据
%/uav1/mavros/local_position/pose    %这个数据滤过波
%/uav1/mavros/local_position/odom    %滤过波，但有时间延迟
vision_pose= select(bag, 'Time',[bag.StartTime bag.EndTime], 'Topic', '/uav1/mavros/local_position/pose');%读取某个话题
stateMsgs_vision_pose= readMessages(vision_pose);
%/uav1/mavros/imu/data
%/uav1/mavros/imu/data_raw   %好像数据一样，但有延迟
IMU_data= select(bag, 'Time',[bag.StartTime bag.EndTime], 'Topic', '/uav1/mavros/imu/data');
stateMsgs_IMU= readMessages(IMU_data);


% state_select_UWB= select(bag_UWB, 'Time',[bag_UWB.StartTime bag_UWB.EndTime], 'Topic', '/uwb_tag_pos');
% stateMsgs_UWB= readMessages(state_select_UWB);
% % dis_select_UWB= select(bag_UWB, 'Time',[bag_UWB.StartTime bag_UWB.EndTime], 'Topic', '/nlink_linktrack_nodeframe3');
% % disMsgs_UWB= readMessages(dis_select_UWB);
% 
ts_vision_pose= timeseries(vision_pose, 'Pose.Position.X','Pose.Position.Y','Pose.Position.Z');
ts_IMU= timeseries(IMU_data, 'LinearAcceleration.X','LinearAcceleration.Y','LinearAcceleration.Z');

% ts_state_UWB= timeseries(state_select_UWB, 'Pose.Position.X','Pose.Position.Y','Pose.Position.Z');
% 
% pose_i_z = ts_vision_pose.Data(204:4754,3);  % 第204个数据对应的时间为1617795609.448063
% pose_U_z = ts_state_UWB.Data(:,3);          % 第一个数据对应的时间为1617795609.438247
% pose_i_x = ts_vision_pose.Data(204:4754,1);
% pose_U_x = ts_state_UWB.Data(:,1);
% pose_i_y = ts_vision_pose.Data(204:4754,2);
% pose_U_y = ts_state_UWB.Data(:,2);
pose_vision_x=ts_vision_pose.Data(:,1);
pose_vision_y=ts_vision_pose.Data(:,2);
pose_vision_z=ts_vision_pose.Data(:,3);

accleration_x=ts_IMU.Data(:,1);
accleration_y=ts_IMU.Data(:,2);
accleration_z=ts_IMU.Data(:,3);
% 
% % plot state in relative time.
t_relative_vision=ts_vision_pose.Time - ts_vision_pose.Time(1,1);
t_relative_IMU=ts_IMU.Time - ts_IMU.Time(1,1);

% t_relative_UWB= ts_state_UWB.Time - ts_state_UWB.Time(1,1); 
% t_relative_icon= ts_vision_pose.Time(204:4754,1) - ts_vision_pose.Time(204,1); 


